#include "Int_Servor.h"

void Int_Servor_Init(void){
    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
}
uint16_t Int_AngleToPulse(float angle)
{
  angle = angle < 0 ? 0 : (angle > 180 ? 180 : angle);
  return (uint16_t)(500 + (angle / 180.0f) * 2000);
}
void Int_Servo_SetAngle(TIM_HandleTypeDef *htim, uint32_t Channel, float angle)
{
  uint16_t pulse = Int_AngleToPulse(angle);
  __HAL_TIM_SET_COMPARE(htim, Channel, pulse);
}
